﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Engine.Physics;
using Microsoft.Xna.Framework;

namespace Engine.Physics
{
    public class Polygon : CollisionShape
    {
        public List<Vector2> Points;
        private List<Vector2> mPoints;
        public List<Vector2> Axes;

        public Polygon(List<Vector2> _points)
        {
            Axes = new List<Vector2>();
            mPoints = _points;
            Points = new List<Vector2>(mPoints);
        }

        private Vector2 GetMaxPointInternal(Vector2 _dir, out int _index)
        {
            float max = Vector2.Dot(Points[0], _dir);
            Vector2 maxPoint = Points[0];
            _index = 0;
            //Project remaining 3
            for (int j = 1; j < Points.Count; j++)
            {
                float val = Vector2.Dot(Points[j], _dir);
                //Update min/max projection on the axis
                if (val > max)
                {
                    max = val;
                    maxPoint = Points[j];
                    _index = j;
                }
            }
            return maxPoint;
        }

        public override Vector2 GetMaxPoint(Vector2 _dir)
        {
            int index;
            return GetMaxPointInternal(_dir, out index);
        }

        public override void GetBestEdge(out Vector2 _max, out Vector2 _e1, out Vector2 _e2, Vector2 _n)
        {
            int index;
            _max = GetMaxPointInternal(_n, out index);

            Vector2 prev = Points[index - 1 < 0 ? Points.Count - 1  : index - 1];
            Vector2 next = Points[(index + 1) % Points.Count];

            Vector2 l = _max - next;
            Vector2 r = _max - prev;

            if (Vector2.Dot(r, _n) <= Vector2.Dot(l, _n))
            {
                _e1 = prev;
                _e2 = _max;
            }
            else
            {
                _e1 = _max;
                _e2 = next;
            }
        } 
        public Vector2 GetMidpoint()
        {
            Vector2 mid = Vector2.Zero;
            foreach (Vector2 point in Points)
                mid += point;
            mid /= Points.Count;
            return mid;
        }

        public void Update(Vector2 _position, float _rotation)
        {
            Points.Clear();
            for (int i = 0; i < mPoints.Count; i++)
            {
                Vector2 point = mPoints[i];
                point = Vector2.Transform(point, Matrix.CreateRotationZ(_rotation));
                point += _position;
                Points.Add(point);
            }
            Axes.Clear();
            for (int i = 0; i < Points.Count; i++)
            {
                Vector2 point1 = mPoints[i];
                Vector2 point2 = mPoints[(i + 1) % Points.Count];

                Vector2 dir = point2 - point1;
                dir = new Vector2(dir.Y, -dir.X);
                dir.Normalize();
                Axes.Add(dir);
            }

            
        }

        public bool Intersects(Polygon _other, ref CollisionInfo _collInfo)
        {
            _collInfo.collding = true;
            _collInfo.willCollide = true;
            Vector2 shortestAxis = Vector2.Zero;
            float shortestProjection = float.PositiveInfinity;

            Vector2[] axes = new Vector2[Axes.Count + _other.Axes.Count];

            for (int i = 0; i < Axes.Count; i++)
            {
                axes[i] = Axes[i];
            }

            for (int i = 0; i < _other.Axes.Count; i++)
            {
                axes[i + Axes.Count] = _other.Axes[i];
            }

            List<Vector2> allCornersA = new List<Vector2>(Points.Count);
            List<Vector2> allCornersB = new List<Vector2>(_other.Points.Count);

            for (int i = 0; i < Points.Count; i++)
            {
                allCornersA.Add(Points[0]);
            }

            for (int i = 0; i < Points.Count; i++)
            {
                allCornersB.Add(_other.Points[0]);
            }
            //this-other test
            for (int i = 0; i < axes.Length; i++)
            {
                /////////////////////////////////////////////////////////////////////
                //Project on the others objects coreners on this objects axis
                float aMax = 0;
                float aMin = 0;
                float[] cornerValuesA = new float[Points.Count];
                ProjectToAxis(Points, axes[i], ref aMin, ref aMax, ref cornerValuesA);

                /////////////////////////////////////////////////////////////////////
                //Project own corners on this axis
                float bMax = 0;
                float bMin = 0;
                float[] cornerValuesB = new float[_other.Points.Count];
                ProjectToAxis(_other.Points, axes[i], ref bMin, ref bMax, ref cornerValuesB);

                /////////////////////////////////////////////////////////////////////

                float gapWidth = GapWidth(aMin, aMax, bMin, bMax);
                //if the mins/max' don't overlap, get out
                if (gapWidth > 0)
                {
                    _collInfo.collding = false;
                    return false;
                }

                if (aMin < bMin)
                {
                    for (int k = 0; k < cornerValuesA.Length; k++)
                    {
                        if (cornerValuesA[k] < bMin)
                            allCornersA.Remove(Points[k]);

                        if (cornerValuesA[k] > bMax)
                            allCornersA.Remove(Points[k]);
                    }

                    for (int k = 0; k < cornerValuesB.Length; k++)
                    {
                        if (cornerValuesB[k] > aMax)
                            allCornersB.Remove(_other.Points[k]);
                    }
                }
                else
                {
                    for (int k = 0; k < cornerValuesB.Length; k++)
                    {
                        if (cornerValuesB[k] < aMin)
                            allCornersB.Remove(_other.Points[k]);

                        if (cornerValuesB[k] > aMax)
                            allCornersB.Remove(_other.Points[k]);
                    }

                    for (int k = 0; k < cornerValuesA.Length; k++)
                    {
                        if (cornerValuesA[k] > bMax)
                            allCornersA.Remove(Points[k]);
                    }
                }

                gapWidth = Math.Abs(gapWidth);

                if (gapWidth < shortestProjection)
                {
                    shortestProjection = gapWidth;
                    shortestAxis = axes[i];

                    Vector2 distance = GetMidpoint() - _other.GetMidpoint();
                    if (Vector2.Dot(distance, shortestAxis) < 0)
                        shortestAxis = -shortestAxis;
                }

            }

            if (_collInfo.willCollide)
            {
                //set projection
                _collInfo.Normal = shortestAxis;
                _collInfo.MTV = shortestAxis * shortestProjection;

                foreach (Vector2 contactA in allCornersA)
                    _collInfo.ContactPoints.Add(contactA + _collInfo.MTV);

                foreach (Vector2 contactB in allCornersB)
                    _collInfo.ContactPoints.Add(contactB - _collInfo.MTV);
            }

            //if got to this point there is a collision
            return _collInfo.willCollide && _collInfo.collding;
        }

        private void ProjectToAxis(List<Vector2> _corners, Vector2 _axis, ref float _min, ref float _max, ref float[] _cornerValues)
        {
            /////////////////////////////////////////////////////////////////////
            //Project on the others objects coreners on this objects axis
            //Project first corner
            float t1 = Vector2.Dot(_corners[0], _axis);

            _min = t1;
            _max = t1;
            _cornerValues[0] = t1;

            //Project remaining 3
            for (int j = 1; j < _corners.Count; j++)
            {
                t1 = Vector2.Dot(_corners[j], _axis);
                _cornerValues[j] = t1;
                //Update min/max projection on the axis
                if (t1 < _min)
                {
                    _min = t1;
                }
                else if (t1 > _max)
                {
                    _max = t1;
                }
            }
        }

        private float GapWidth(float _t1Min, float _t1Max, float _t2Min, float _t2Max)
        {
            if (_t1Min < _t2Min)
                return _t2Min - _t1Max;
            else
                return _t1Min - _t2Max;
        }
    }
}
